function [q1,q2,q3,q1_dot,q2_dot,q3_dot,q1_doot,q2_doot,q3_doot] = path_plan_3(qi,qf,tf,ts)
%使用三样条插值规划机械臂在工作空间中的路径，默认了初始角速度和目标角速度都为0，如果角速度不是0更改a_0,a_1,a_2,a_3表达式即可
%单位使用弧度rad
%输入：
%   qi:初始关节角，三维向量
%   qf:目标关节角，三维向量
%   tf:机械臂总共需要运动的时间
%   ts:即time step，每步时间间隔

%输出：
%   q1:即theta_1,关节1角度，是一个时间序列,该序列共有1:ts:tf个数据点
%   q2:同理
%   q3:同理
%   q1_dot:关节1的角速度
%   q2_dot:关节2的角速度
%   q3_dot:关节3的角速度
%   q1_doot:关节1的角加速度
%   q2_doot:关节2的角加速度
%   q3_doot:关节3的角加速度

    
    t = 0:ts:tf;

    %计算样条曲线参数
    a_0 = qi;
    a_1 = zeros(size(qi)); %这里默认初始关节角速度为0
    a_2 = (3/tf^2)*(qf-qi);
    a_3 = (-2/tf^3)*(qf-qi);
    
    q1 = a_0(1)+a_1(1)*t+a_2(1)*t.^2+a_3(1)*t.^3; %计算出关节角度曲线
    q2 = a_0(2)+a_1(2)*t+a_2(2)*t.^2+a_3(2)*t.^3; 
    q3 = a_0(3)+a_1(3)*t+a_2(3)*t.^2+a_3(3)*t.^3; 
    
    q1_dot = a_1(1)+2*a_2(1)*t+3*a_3(1)*t.^2;
    q2_dot = a_1(2)+2*a_2(2)*t+3*a_3(2)*t.^2;
    q3_dot = a_1(3)+2*a_2(3)*t+3*a_3(3)*t.^2;
    
    q1_doot = 2*a_2(1) + 2*3*a_3(1)*t;
    q2_doot = 2*a_2(2) + 2*3*a_3(2)*t;
    q3_doot = 2*a_2(3) + 2*3*a_3(3)*t;


end
